#include <iostream>
#include <mysql/mysql.h>
#include "smallcardb.h"
#include "odom_slam.h"
#include "odom_gps.h"
#include "odom_imu.h"
#include <list>
#include <string>
#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <queue>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <time.h>
#include <sys/time.h>
#include <stdlib.h>
#include <functional>
#include "small_car_map.h"
#include <math.h>
#include "navigation_msg/odom.h"
#include "didi_msgs/Pose2DStamped.h"
#include "ros/ros.h"
#include "std_msgs/Float32MultiArray.h"

#define MEASURE_DISTANCE 58800
#define MEASURE_PULSE    798



using namespace std;
namespace small_car_map {

   small_car_map::small_car_map():
       imu_x(0.0),
       imu_y(0.0),
       imu_angle(0.0),
       slam_x(0.0),
       slam_y(0.0),
       slam_angle(0.0),
       gps_x(0.0),
       gps_y(0.0),
       time_count(0),
       rev_imu(false)
       //my_smallcar_db(NULL)
   {


       my_smallcar_db.car_db_init();
       printf("build small_car_map success \r\n");
       pub_odom_data  = odom_cab.advertise<didi_msgs::Pose2DStamped>
               ("test_traj_display", 10);

   }

    small_car_map::~small_car_map(){

        printf("destroy small_car_map success \r\n");

    }

    void small_car_map::OdomDataCallback(const  navigation_msg::odom& odom_message){

           int32_t Left_Speed=0;
           int32_t Right_Speed=0;
           float  l_dis_s_seg=0;
           float  r_dis_s_seg=0;
           float  wheel_dis_s_seg=0;
           float wheel_dis_x_seg=0;
           float wheel_dis_y_seg=0;

           double rad=0,Dsin=0,Dcos=0;
           Left_Speed=odom_message.left_speed;
           Right_Speed=odom_message.right_speed;
           //imu_angle=odom_message.imu_yaw;


           l_dis_s_seg=(float)(Left_Speed*MEASURE_DISTANCE/MEASURE_PULSE); //分辨率0.01MM,放大100倍
           r_dis_s_seg=(float)(Right_Speed*MEASURE_DISTANCE/MEASURE_PULSE); //右边轮子线段距离
           wheel_dis_s_seg=(l_dis_s_seg+r_dis_s_seg)/2;
           rad=(-(double)imu_angle*3.1415/1800);//计算弧度
           Dsin= sin(rad);
           Dcos= cos(rad);
           wheel_dis_x_seg = Dcos*wheel_dis_s_seg/100;
           imu_x += wheel_dis_x_seg;
           wheel_dis_y_seg = Dsin*wheel_dis_s_seg/100;
           imu_y += wheel_dis_y_seg;



           /*odom_imu *robot_run=new odom_imu(ros::Time::now().toSec(),imu_angle,wheel_dis_s_seg,imu_x,imu_y);
           my_smallcar_db.Add_odom_imu(robot_run);
           free(robot_run);*/

           //printf("imu_x:%f imu_y:%f  imu_angle:  %f\r\n",imu_x,imu_y,imu_angle);

           rev_imu=true;


    }

    void small_car_map::grid_transf_slam(int distance, float angle){




    }

    void  small_car_map::imu_map_display(){

         didi_msgs::Pose2DStamped p;
         p.header.frame_id = "/map";
         p.header.stamp = ros::Time::now();

         p.trajpoint.x =imu_x;
         p.trajpoint.y = imu_y;
         p.trajpoint.theta = imu_angle;

         pub_odom_data.publish( p );
    }


   void small_car_map::ImuDataCallback(const std_msgs::Float64MultiArrayConstPtr& imu_message){

        imu_angle=imu_message->data[0];
        //printf("imu_angle:%f\r\n",imu_angle);

    }

    void small_car_map::map_run(){


            ros::Subscriber odom_imu=odom_cab.subscribe("serial/odom_data",
                10,&small_car_map::OdomDataCallback,this);

            ros::Subscriber imu_sensor=odom_cab.subscribe("sensor/imu",
                5,&small_car_map::ImuDataCallback,this);

             ros::Rate loop_rate(100);

             while (odom_cab.ok())
             {

                 //if(rev_imu) map_display();
                 loop_rate.sleep();
                 ros::spinOnce();
             }


    }


}
